Mobile working machine comprising a position control device of a working arm, and method for controlling the position of a working arm of a mobile working machine

ABSTRACT

A mobile working machine includes a working arm which is mounted in an articulated manner, by a first end, on a revolving superstructure of the working machine, and a tool which is mounted in a displaceable manner on a second end of the working arm. The mobile working machine includes a position control device of a working arm. At least one inclination sensor is arranged on the revolving superstructure and at least one inclination sensor is arranged on the working arm. Also, at least one rotation rate sensor is arranged on the working arm. The working machine further includes a calculation unit for processing the signals of the at least one inclination sensor, of at least one additional inclination sensor, and of at least one rotational rate sensor.

The invention relates to a mobile working machine, for example anexcavator, a truck with a superstructure, or an agricultural andforestry device comprising at least one working arm. Working arms ofsuch working machines usually have a plurality of segmentsinterconnected in an articulated fashion, a tool such as a bucket, agripper or a hammer being fitted on a segment, also called a shank.

The current position and attitude of the working arm and, in particular,also of the tool is frequently indicated to the operator of such aworking machine on a display, the operator thereby being enabled tocarry out work according to accurately prescribed plans, and receivesdirect feedback relating to achieved levels, lengths, depths orinclinations, for example of a bulk material or soil which has beenmoved, or of the underlying ground which is to be shaped or has alreadybeen shaped.

Such operator displays are known, for example, from DE 201 16 666 U1 andU.S. Pat. No. 5,854,988 A.

In this case, the position and the attitude of the tool are frequentlydetermined, partly because of the good retrofitting possibility, byinclination sensors that are fitted on the individual segments of theworking arm. The position and attitude of the tool can then becalculated from the inclinations of the individual arm segments via theknown geometric relationships of the kinematic chain comprising, forexample, adjustable boom, boom, shank and tool.

Since, however, sensors based on the measurement principle of inertiasuch as, for example, gravitation-sensitive pendulums, are used asinclination sensors, they are frequently also sensitive to accelerationsdue to shocks and vibrations such as occur unavoidably when such workmachines are used. Such motion-induced accelerations can substantiallydisturb or temporarily disable the measurement of the position andattitude of the tool. A known measure for suppressing these disturbancesconsists of a lowpass filtering of the sensor signals such thatmotion-induced accelerations outside the useful frequency band aresuppressed.

However, this is attended by a number of disadvantages: firstly,accelerations within the useful frequency band cannot be suppressed inthis way, while secondly the lowpass filtering causes a temporal delayof up to a few hundred milliseconds which is, however, accepted, becauseit still enables an adequately accurate position display and does nothinder the manual operation of the working arm.

It is known from other technical fields to make use of a combination ofacceleration-based inclination and rotation rate sensors to control theposition of, for example, a robot, an aerodynamic vehicle or a vehicle.WO 01/57474 A1 discloses such a method, in which a quaternionrepresentation is used to calculate a position.

It is an object of the invention to specify a mobile working machinecomprising a device for determining position that has adequately smalldelay times such that it can be used not only to display the position,but also to control the position of any working arm of the workingmachine. It is, however, not intended to restrict the dynamics of theworking arm in this case. It is a further object of the invention tospecify a method for such control of position.

According to the invention, this object is achieved with the aid of thesubject matter of the independent patent claims. Advantageousdevelopments of the invention are the subject matter of the dependentpatent claims.

A mobile working machine according to the invention comprising aposition control device of a working arm has a working arm that isarranged in an articulated fashion, by a first end, on a revolvingsuperstructure of the working machine. A tool is movably arranged on asecond end of the working arm.

The working machine further has a number of sensors, specifically atleast one inclination sensor arranged on the revolving superstructure,at least one further inclination sensor arranged on the working arm, andat least one rotation rate sensor arranged on the working arm. Theworking machine further comprises an arithmetic logic unit forprocessing the signals of the at least one inclination sensor, of the atleast one further inclination sensor and of the at least one rotationrate sensor. Of course, however, the arithmetic logic units can also beaccommodated in the sensor modules fastened on the arm.

In accordance with a basic idea of the invention, it should be possibleto achieve delay times of at most approximately 250 ms for real-timecontrol. However, this value can also vary as a function of the hardwareinstalled for the working arm, and of the purpose of use. Lowpassfiltering of the measured values, which is therefore accompanied bysignal delay, should therefore be dispensed with. On the other hand,inclination sensors should be used as before, because these arerelatively cost effective and robust and, moreover, can easily beretrofitted and are therefore particularly suitable for use on mobileworking machines. In order to render the inclination measurement lesssusceptible to the already described disturbances due to motion-inducedaccelerations, additional use is made of rotation rate sensors. Thelatter have a high dynamic accuracy, although they are attended byproblems such as offsets and noise. Rotation rate sensors that are usedin avionic navigation and are based on the Sagnac effect as well as, forexample, on the use of fiber-optical gyros are certainly very accurate.By contrast, micromechanical rotation rate sensors are, however, muchless cost-effective and, above all, also more robust and thereforebetter suited for use on mobile working machines. Because of said driftproblems with such rotation rate sensors, the latter are combined withinclination sensors, in accordance with the invention.

This solution has the advantage that it is tailored accurately to use onmobile working machines, because it exhibits sufficient accuracy forreal-time control of the position of the working arm or of a tool fittedthereon, but at the same time can be implemented cost-effectively and isvery robust and little prone to error. Real-time control is thereforepossible by combining inclination sensors with a very good staticaccuracy and rotation rate sensors with a very good dynamic accuracywith the aid of a data fusion or estimation algorithm.

The working arm can have a number of segments interconnected in anarticulated fashion, the tool being fitted, for example, on the end ofthe last segment. In each case, the working arm comprises at least onesegment.

In one embodiment, a further inclination sensor and a rotation ratesensor are arranged on each segment.

In an alternative embodiment, two further inclination sensors and arotation rate sensor are arranged on each segment. In the case of thisembodiment, it is possible not only to measure a spatial component ofthe acceleration (for example in an x-direction), but it issimultaneously possible to measure two components (for example in x- andz-directions), and this improves the sensitivity of the measurement inspecific inclination angle ranges.

The inclination sensors can be based on different measurement principlesand have, for example, pendulum bodies and/or refracting liquid mirrors.They can also be designed as capacitive or conductometric inclinationsensors, but preferably as micromechanical acceleration sensors.

The rotation rate sensors are designed, in particular, asmicromechanical sensors.

The position control device is suitable for use in mobile workingmachines such as, for example excavators, telescopic loaders, excavatorloaders, wheeled loaders, loading cranes or forestry machines.

In accordance with a further aspect of the invention, a method isspecified for controlling the position of a working arm of a mobileworking machine, the determination of position having the followingsteps: acceleration α acting on the working arm is determined. This canbe performed, for example, in all three spatial directions, threecomponents a_(x), a_(y) and a_(z) being measured with the aid of threeacceleration sensors. However, it can also suffice to measure only theacceleration in x- and z-directions. Assuming a working arm at rest, afirst value θ_(s) for the inclination angle θ is calculated from theacceleration α acting on the working arm, the inclination angle θ beingdefined as an angle by which the working arm is inclined about a y-axis.

In this case, the following relationships are valid for the accelerationsensors fitted on the working arm, since a coordinate transformationfrom the rest system of the segments of the working arm or of thesensors into the rest system of the revolving superstructure isundertaken in order to determine the inclination angle θ:

a _(x,S) ={dot over (v)} _(x)−ω_(z) v _(y)+ω_(y) v _(z) −g sin θ

a _(y,S) ={dot over (v)} _(y)+ω_(s) v _(z)−ω_(x) v _(z) +g sin φ cos θ

a _(z,S) ={dot over (v)} _(z)−ω_(y) v _(z)+ω_(x) v _(y) +g cos φ cosθ  (1)

Here, a denotes the measured values of the sensors in the directions ofthe fixed-body axes x, y and z, and the index S characterizes therespective sensor. v denotes the velocity of the working arm, ω itsangular velocity, and φ and θ the so-called Euler angles of roll anglesand inclination angles. g denotes the acceleration due to gravity.

The respective first terms of the equations in (1) describe theacceleration resulting from a transformation of the entire workingmachine or of the working arm or segment, while the respective middleterms describe the acceleration resulting from a rotation of the entireworking machine or of the working arm or segment. These terms thusrespectively describe “disturbances” by contrast with the respectivelast term, because they relate to accelerations that do not result fromthe influence of the terrestrial gravitational field, and therefore donot reproduce the inclination of the corresponding sensor in thegravitational field.

Since, as emerges from the second row of (1), the acceleration sensormeasuring in a y-direction measures only the abovementioned disturbanceswhen roll angle φ vanishes, it is possible, if appropriate, also todispense with it when the static case (working arm at rest) is assumedfor the calculation of the first value θ for the inclination angle θ.The acceleration signals described by the first and third rows of (1)are substantially disturbed by linear movements in x- and z-directionsand by rotations about the y-axis (centripetal and coriolis terms).

The following procedure is adopted, for example, in order to calculatethe first value θ for the information angle θ from the measured valuesof the acceleration sensors:

Assuming a working arm at rest, that is to say a static case, it followsfrom (1) that:

a _(x,S) =−g sin θ_(s)

a _(y,S) =φg cos θ_(s)

a _(z,S) =g cos θ_(s)  (2),

from which is derived the desired first value θ for the inclinationangle θ in accordance with

$\begin{matrix}{\mspace{79mu} {{\tan \text{?}} = {{- {\frac{\text{?}}{a_{i,S}}.\text{?}}}\text{indicates text missing or illegible when filed}}}} & (3)\end{matrix}$

As follows from (2), it would also be possible to make a determinationfor an arm θ at rest by way of only one sensor in accordance with

$\begin{matrix}{\mspace{79mu} {{\sin \text{?}} = {{- {\frac{\text{?}}{g}.\text{?}}}\text{indicates text missing or illegible when filed}}}} & (4)\end{matrix}$

However, this is attended by the disadvantage that the range of valuesof θ is restricted to the interval −90° . . . 90°. At least for theshank of a working machine, that is to say for that arm segment on whichthe tool is fastened, and for the tool itself, this range of values istoo small for describing the realistic relationships. In addition, thesensitivity of the measurement is low in this case because of theshallow gradient of the sine function near −90° and 90°. Instead ofthis, it is possible to apply to (2) the arctan function with twoarguments (“a tan 2”) with a measurement value between −180° and 180°.

The assumption of a working arm at rest has been made here in order todetermine the first value θ for the inclination angle θ. Given a workingarm which has been moved and a possibly revolving superstructure of theworking machine, the disturbances owing to the first and middle terms in(1) quickly become so large that determination of the inclination anglewith the aid of (3) no longer delivers sufficiently accurate results.This procedure is therefore not adequate for real-time control of theposition of the working arm.

For this reason, an angular velocity ω of the working arm, specificallyat least one component ω_(y) of the rotation about the y-axis, ismeasured in addition. Specifically, there is further known from rigidbody kinematics the following system of equations which relates therotation rate or angular velocity ω to the Euler angles θ, φ and ψ andto the time derivatives thereof:

$\begin{matrix}{\mspace{79mu} {{\overset{.}{\vartheta} = {{\cos \; {\varphi \cdot \omega_{y}}} - {\sin \; {\varphi \cdot \text{?}}}}}\mspace{79mu} {\overset{.}{\varphi} = {\text{?} + {\tan \; {{\vartheta sin\varphi} \cdot \omega_{y}}} + {\tan \; {\vartheta cos}\; {\varphi \cdot \text{?}}}}}\mspace{79mu} {{\overset{.}{\psi} = {{\frac{\sin \; \varphi}{\cos \; \vartheta}\omega_{y}} + {\frac{\cos \; \varphi}{\cos \; \vartheta}\text{?}}}},,{\text{?}\text{indicates text missing or illegible when filed}}}}} & (5)\end{matrix}$

from which it follows that

$\begin{matrix}{\mspace{79mu} {{\overset{.}{\vartheta} = \omega_{y}}\mspace{79mu} {\overset{.}{\varphi} = {\text{?} + {\tan \; {\vartheta \cdot \text{?}}}}}\mspace{79mu} {{\overset{.}{\psi} = \frac{\text{?}}{\cos \; \vartheta}},{\text{?}\text{indicates text missing or illegible when filed}}}}} & (6)\end{matrix}$

for vanishing roll angles φ.

It is thereby possible to obtain a second value θ_(d) for theinclination angle θ in the dynamic case by integrating the angularvelocity ω_(y) over a period t:

θ_(d)=θ₀+∫ω_(y) dt.

However, the problem arises in this case that, for example, offseterrors and sensor noise are continuously integrated, which results evenafter a relatively short time, in deviations from the true inclinationangle θ which are so large that the accuracy of the measurement does notmeet the requirements placed thereon. This is the case at least when useis made of micromechanical rotation rate sensors. Other sensors such as,for example, fiber optic gyro sensors would deliver a greater, and inmany cases sufficient, accuracy. However, they have the disadvantage ofhigh costs and a relatively low robustness, so that they are not wellsuited for, use on mobile working machines.

In order now to make use of the available means of the inclinationsensors, on the one hand, which give a good description of the staticcase, and the rotation rate sensor, on the other hand, which gives agood description of the dynamic case, but exhibits drift problems overrelatively large spaces of time, to obtain a sufficiently accurate valuefor the inclination angle θ, an estimation algorithm is employed so asto obtain an estimate θ for the true inclination angle d from the twovalues θ_(s) and θ_(d). Starting from the accelerations a_(x,S) anda_(z,S) measured in the x- and z-directions, the a tan 2 function isused in accordance with (3) to calculate the angle θ_(s) for the staticcase, which is then used as intermediate variable for integrating therotation rate ω_(y). To this end, the difference between the result ofintegration and the intermediate value is fed back into the estimationalgorithm. It is also possible in this way to reduce erroneous initialvalues θ₀.

It is possible for this purpose to make use of estimation algorithms perse, for example Kalman filters, or to use observation methods known fromautomatic control engineering that can also additionally estimate ifappropriate the offset error of the rotation rate sensor.

The result is an estimate θ for the true inclination angle θ.

The inventive method has the advantage that it is possible with the aidof only three sensors—two acceleration sensors and one rotation ratesensor—to achieve a determination of the position of the working arm orof a specific point of the working arm, for example the suspension ofthe tool, with an accuracy that enables real-time control.

In one embodiment, the working arm has a plurality of segments beingconnected in an articulated fashion, and the determination of estimatesθ for the inclination angles is carried out individually for eachsegment, the index i standing for the corresponding segment.

In one embodiment, the position and the attitude of a tool fitted on theworking arm of the mobile working machine is calculated from θ, and saidtool is controlled if appropriate.

However, it is also possible to calculate the deflection of at least onehydraulic cylinder assigned to the working arm of the mobile workingmachine from θ and to control said deflection if appropriate.

Exemplary embodiments of the invention are explained in more detailbelow with the aid of the attached figures, in which:

FIG. 1 is a schematic of a mobile working machine, designed as anexcavator, in accordance with one embodiment of the invention;

FIG. 2 is a schematic of the geometric relationships on a working arm ofthe working machine in accordance with FIG. 1;

FIG. 3 shows a graph of the primary signal flow for embodiments of thecontrol position in accordance with the invention;

FIG. 4 shows a diagram of inclination signals disturbed by movements;

FIG. 5 shows a diagram of the determination of inclination angle withthe aid of lowpass filtering;

FIG. 6 shows a diagram of the drift-affected determination ofinclination angle with the aid of a rotation rate sensor; and

FIG. 7 is a schematic block diagram for estimating the inclination anglein accordance with the invention.

Identical parts are provided with the identical reference numerals inall the figures.

FIG. 1 is a schematic of a mobile working machine 1 designed as anexcavator. In this embodiment, the working machine 1 has a base part 2that stands or moves on an underlying ground 5, and a revolvingsuperstructure 3 that can be rotated about a vertical axis 4.

Connected to the revolving superstructure 3 in an articulated fashion isa working arm 6 that comprises a first segment 7 and a second segment 8which are likewise interconnected in an articulated fashion. A tool 9 iscoupled on the second segment 8, the fastening point of the tool 9 alsobeing referred to as Tool Center Point (TCP) 10. Hydraulic cylinders 11that respectively define an angle θ_(i) are arranged between therevolving superstructure and the first segment 7, between the segments 7and 8 and between the second segment 8 and the tool 9.

Arranged on the revolving superstructure 3 is a sensor unit 13, on thefirst segment 7 a sensor unit 14, on the second segment 8 a sensor unit15 and on the tool 9 a sensor unit 16. In this embodiment, each sensorunit 13, 14, 15 and 16 comprises two inclination sensors and a rotationrate sensor, the two inclination sensors measuring accelerations in thex- and z-directions, and the rotation rate sensor measuring an angularvelocity of the rotation about the y-axis.

In an embodiment that is not shown, it is also possible to dispense withone of the inclination sensors per sensor unit, and the sensor unitassigned to the revolving superstructure 3 can also have only one,inclination sensor, but no rotation rate sensor if relatively fewdisturbances caused by movements of the revolving superstructure are tobe expected.

The measured data of the sensor units 13, 14, 15 and 16 are fed to anarithmetic logic unit 17 of the mobile working machine 1, whichundertakes to determine and control a position therefrom, particularlyregarding the TCP 10, and to determine and control the attitude of thetool 9. The arithmetic control unit for determining position can,however, also be located in the sensor modules that are mounted on thearm.

FIG. 2 shows a schematic of the geometrical relationships on the workingarm 6 of the mobile working machine 1 in accordance with FIG. 1.

The revolving superstructure 3 is inclined at an angle θ₁, the firstsegment at an angle θ₂, the second segment at an angle θ₃, and the tool9 at an angle θ₄ with respect to the perpendicular to the direction ofacceleration due to gravity g. The distances l₂ and l₃ between the firstfulcrum 18 and the second fulcrum 19 or between the second fulcrum 19and the TCP 10, which represent the lengths of the first segment 7 andof the second segment 8, are known, and so the position of the TCP 10and the attitude of the tool 9 can be calculated with the aid of adetermination of the inclination angle θ_(i). That is to say, thegeometrical (design) data of the arm segments are known in advance.

FIG. 3 shows a graph of the primary signal flow for embodiments of thecontrol position in accordance with the invention.

In this case, in a first step the accelerations are measured and therotation rates at all elements of the kinematic chain are measured, thatis to say at the revolving superstructure 3, at the first segment 7, atthe second segment 8 and at the tool 9. The inclination angles θ_(i) aredetermined dynamically from these measured values.

Finally, as illustrated in conjunction with FIG. 2, it is possible touse the inclination angles θ_(i) to determine the position and theattitude of the tool 9. Alternatively or in addition, however, it isalso possible to calculate the deflection of the hydraulic cylinder 11therefrom.

FIG. 4 shows a diagram of inclination signals disturbed by movements,such as results when the inclination angle is calculated solely fromequation (3), that is to say exclusively with the aid of inclinationsensors. Because of motion-induced accelerations, substantial deviationsresult in part between the calculated and the true inclination angles.

FIG. 5 shows a diagram of the determination of the inclination anglefrom equation (3) with the aid of lowpass filtering, the time delay,caused by the lowpass filtering being made clear.

FIG. 6 shows a diagram of the drift-affected determination ofinclination angle with the aid of a rotation rate sensor in accordancewith equation (7), that is to say without the aid of inclinationsensors.

FIG. 7 shows a schematic block diagram for estimating the inclinationangle θ in accordance with the invention for an individual sensor unit,the measured values a_(x,S) and a_(z,s) of the inclination sensors ofthe sensor unit firstly being used to calculate a static inclinationangle θ_(s), and the measured value ω_(y) of the rotation rate sensor ofthe same sensor unit being integrated in order to calculate a dynamicinclination angle θ_(d). Subsequently, the difference between the staticand dynamic inclination angles is fed back into the estimation algorithmin order to obtain an estimate θ for the inclination angle.

Methods known per se such as, for example, Kalman filters or observationmethods or methods derived or modified therefrom are used in this caseas estimation algorithm.

LIST OF REFERENCE NUMERALS

-   1 Working machine-   2 Base part-   3 Revolving superstructure-   4 Axis-   5 Underlying ground-   6 Working arm-   7 First segment-   8 Second segment-   9 Tool-   10 Tool center point-   11 Hydraulic cylinder-   13 Sensor unit-   14 Sensor unit-   15 Sensor unit-   16 Sensor unit-   17 Arithmetic logic unit-   18 First fulcrum-   19 Second fulcrum

1. A mobile working machine comprising: a position control device of aworking arm including a working arm that is arranged in an articulatedfashion, by a first end, on a revolving superstructure of the workingmachine, a tool that is movably arranged on a second end of the workingarm, at least one inclination sensor arranged on the revolvingsuperstructure, at least one further inclination sensor arranged on theworking arm, at least one rotation rate sensor arranged on the workingarm, and an arithmetic logic unit for processing the signals of the atleast one inclination sensor, of the at least one further inclinationsensor, and of the at least one rotation rate sensor.
 2. The mobileworking machine as claimed in claim 1, wherein the working arm has anumber of segments interconnected in an articulated fashion.
 3. Themobile working machine as claimed in claim 2, wherein a furtherinclination sensor and a rotation rate sensor are arranged on eachsegment.
 4. The mobile working machine as claimed in claim 2, whereintwo further inclination sensors and a rotation rate sensor are arrangedon each segment.
 5. The mobile working machine as claimed in claim 1,wherein the inclination sensors have pendulum bodies.
 6. The mobileworking machine as claimed in claim 1, wherein the inclination sensorshave refracting liquid mirrors.
 7. The mobile working machine as claimedin claim 1, wherein the inclination sensors are designed as capacitive,in particular fine mechanical or micromechanical, inclination sensors.8. The mobile working machine as claimed in claim 1, wherein theinclination sensors are designed as conductometric inclination sensors.9. The mobile working machine as claimed in claim 1, wherein therotation rate sensors are designed as micromechanical sensors.
 10. Themobile working machine as claimed in claim 1, wherein the mobile workingmachine is designed as an excavator.
 11. The mobile working machine asclaimed in claim 1, wherein the mobile working machine is designed as atelescopic loader.
 12. The mobile working machine as claimed in claim 1,wherein the mobile working machine is designed as an excavator loader.13. The mobile working machine as claimed in claim 1, wherein the mobileworking machine is designed as a wheeled loader.
 14. The mobile workingmachine as claimed in claim 1, wherein the mobile working machine isdesigned as a forestry machine.
 15. The mobile working machine asclaimed in claim 1, wherein the mobile working machine is designed as aloading crane.
 16. A method for controlling the position of a workingarm of a mobile working machine, the determination of the position ofthe working arm comprising: determining an acceleration acting on theworking arm; calculating a first value for an inclination angle from theacceleration acting on the working arm, assuming a working arm at rest,the inclination angle being defined as an angle at which the working armis inclined about a y-axis; measuring an angular velocity of the workingarm about the y-axis; integrating the angular velocity over a period inorder to obtain a second value for the inclination angle and using anestimation algorithm in order to obtain from the first value and fromthe second value an estimate for the inclination angle.
 17. The methodas claimed in claim 16, wherein the working arm has a plurality ofsegments interconnected in an articulated fashion, and the determinationof estimates for the inclination angles is carried out individually foreach segment.
 18. The method as claimed in claim 16, wherein theposition and the attitude of a tool fitting on the working arm of themobile working machine is calculated from the estimate for theinclination angle.
 19. The method as claimed in claim 16, wherein thedeflection of at least one hydraulic cylinder assigned to the workingarm of the mobile working machine is calculated from the estimate forthe inclination angle.
 20. The method as claimed in claim 16, wherein acoordinate transformation from the rest system of the segments of theworking arm into the rest system of the revolving superstructure of themobile working machine is undertaken in order to determine theinclination angle.